﻿using UnityEngine;
using System.Collections;
using ROSBridgeLib;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using CnControls;
using UnityEngine.UI;
public class RosTest1 : MonoBehaviour {

    private ROSBridgeWebSocketConnection ros = null;    
   
    private ROSBridgeSubscriber odomSubscriber;

    private SimpleJoystick joystick;



    [SerializeField]
    Slider maxLinearSpeed;

    [SerializeField]
    Slider maxAngularSpeed;


  

    void Start () {



        Connect();

       


    }



    void Connect()
    {

        ros = new ROSBridgeWebSocketConnection ("ws://192.168.1.38", 9090);



        ros.AddPublisher (typeof(TeleopPublisher));

        // ros.AddSubscriber(typeof(OdomSubscribler));

        ros.Connect ();

    }



    void Update () {




        Vector2 inputVector = new Vector2(CnInputManager.GetAxis("Horizontal"), CnInputManager.GetAxis("Vertical"));

        TwistMsg msg = new TwistMsg (new Vector3Msg(inputVector.y*maxLinearSpeed.value,0.0, 0.0), new Vector3Msg(0.0, 0.0, -inputVector.x*maxAngularSpeed.value));

        ros.Publish (TeleopPublisher.GetMessageTopic(), msg);
       
        ros.Render ();

    
    }

    // extremely important to disconnect from ROS. OTherwise packets continue to flow
    void OnApplicationQuit() {
        
        if(ros!=null)
        {
            ros.Disconnect ();
        }
    }

}
